#ifndef ___DISTANCE___
#define ___DISTANCE___

#define print(str) (Uart_Puts(COMM_BT, str))
#define DISTANCESIZE 3
extern char outbuf[100];
int distanceDisplay = 0;
int soundCnt = 0;
int distance[DISTANCESIZE] = {0};

void updateDistance(int d, int chanel)
{
  int rank;
  switch (chanel)
  {
  case MFS1:
    rank = 0;
    break;
  case MFS2:
    rank = 1;
    break;
  case MFS3:
    rank = 2;
    break;
  default:
    exit(6);
  }
  if (soundCnt == 0)
  {
    distance[rank] = (d << 8);
    soundCnt ++;
  }
  else if (soundCnt == 1)
  {
    distance[rank] += d;
    if (distanceDisplay)
    {
      int resident = distance[rank] % 1000;
      if (resident / 10 == 0)
        sprintf(outbuf, "\n\rdistance%d: %d.00%d m\n\r", rank, distance[rank] / 1000, resident);
      else if (resident / 100 == 0)
        sprintf(outbuf, "\n\rdistance%d: %d.0%d m\n\r", rank, distance[rank] / 1000, resident);
      else 
        sprintf(outbuf, "\n\rdistance%d: %d.%d m\n\r", rank, distance[rank] / 1000, resident);
      print(outbuf);
    }
    soundCnt = 0;
  }
}
#endif